#ifndef imu2euler_h
#define imu2euler_h

#include <ros/ros.h>
#include <geometry_msgs/Quaternion.h>  // 四元素消息
#include <tf2/LinearMath/Quaternion.h> // tf四元素
#include <tf2/LinearMath/Matrix3x3.h>  // tf2_q2Mat2euler
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // tf2ToMsg


#include <tf2_ros/transform_broadcaster.h>  // 广播器
#include <geometry_msgs/TransformStamped.h> // 位姿消息

#include <sensor_msgs/Imu.h>

#include <Eigen/Dense>

class Imu2Euler
{
public:
    Imu2Euler();
    ~Imu2Euler(){};
    void callback(const sensor_msgs::ImuConstPtr &msg_imu);
    void acc2Euler(double ax, double ay, double az, double t);   // 加速度投影
    void gyr2Euler(double gx, double gy, double gz, double t);   // 角速度积分
    void quat2Euler(double gx, double gy, double gz, double t);  // 四元数积分
    void imu2Pose(Eigen::Vector3d& acc,Eigen::Vector3d& gyr, double t);

private:
    ros::NodeHandle n_;
    std::string topic_imu_;
    ros::Subscriber sub_imu_;
    
    tf2::Quaternion q0_;
    double last_roll_, last_pitch_, last_yaw_, last_time_;
    double cur_roll_, cur_pitch_, cur_yaw_, cur_time_;
    bool first_data_;

    // geometry_msgs::TransformPtr tf_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    
    ros::Publisher pub_acc_euler_, pub_gyr_euler_, pub_ch110_euler_;
    geometry_msgs::PointStamped msg_point_;

    // 四元数积分用到的变量
    bool first_q_;
    tf2::Quaternion last_q_, cur_q_;
    double last_q_t_, cur_q_t_;

    // 航迹推算
    bool first_PVQ_;
    Eigen::Vector3d last_P_, cur_P_;
    Eigen::Vector3d last_V_, cur_V_;
    Eigen::Vector3d last_Q_, cur_Q_;
    double last_PVQ_t_, cur_PVQ_t_;
    
};



#endif